//#define USE_ARTAG

using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Net;
using Microsoft.Xna.Framework.Storage;

using GoblinXNA;
using GoblinXNA.Graphics;
using GoblinXNA.SceneGraph;
using Model = GoblinXNA.Graphics.Model;
using GoblinXNA.Graphics.Geometry;
using GoblinXNA.Device.Capture;
using GoblinXNA.Device.Vision;
using GoblinXNA.Device.Vision.Marker;
using GoblinXNA.Device.Util;
using GoblinXNA.Physics;
using GoblinXNA.Physics.Newton1;
using GoblinXNA.Helpers;

using GoblinXNA.UI;
using GoblinXNA.Device.Generic;//keyboard



static class MENU
{
    public const int BACK = 0;
    //Heirarchy 1               
    public const int CREATE = 1;
    public const int MANIPULATE = 2;
    public const int SWITCHMODE = 3;
    //Heirarchy 2 - CREATE              
    public const int CUBE = 4;
    public const int CYLINDER = 5;
    public const int WALL = 13;
    //Heirarchy 2 - MANIPULATE          
    public const int TRANSLATE = 6;
    public const int SCALE = 7;
    public const int ROTATE = 8;
    //Heirarchy 3 - MANIPULATE          
    public const int XAXIS = 9;
    public const int YAXIS = 10;
    public const int ZAXIS = 11;

    public const int AXES = 12;
    public const int MAIN = 0;


}


namespace AugmentedRealityTeam
{
    /// <summary>
    /// This tutorial demonstrates how to use both the ALVAR and ARTag optical marker tracker 
    /// library with our Goblin XNA framework. Please read the README.pdf included with this 
    /// project before running this tutorial. If you're using ARTag, please remove calib.xml
    /// from the solution explorer so that you can successfully build the project. 
    /// </summary>
    /// 	    // this is a class that I made to hold all the boolean variables that we use so that it is convenient ... Neerja		
    // please transfer your previous ones to here, since it is static use the class names Flags.whatever		
    class Flags
    {
        public static Boolean createMode = false;
        public static Boolean createWallMode = false;
        public static Boolean manipulateMode = false;
        public static Boolean scaleX = false;
        public static Boolean scaleY = false;
        public static Boolean scaleZ = false;
        public static Boolean translateX = false;
        public static Boolean translateY = false;
        public static Boolean translateZ = false;
        public static Boolean rotateX = false;
        public static Boolean rotateY = false;
        public static Boolean rotateZ = false;

    }
    public class Game1 : Microsoft.Xna.Framework.Game
    {
        GraphicsDeviceManager graphics;
        Scene scene;
        MarkerNode groundMarkerNode, miniMapMarkerNode, groundReferenceMarkerNode, speedMarkerNode;
        GeometryNode groundNode, boxNode, realgroundNode;
        GeometryNode[] sceneObjects;
        TransformNode[] sceneObjectsTrans;
        GeometryNode[] minimapObjects;
        TransformNode[] minimapObjectsTrans;
        GeometryNode[] minimapWalls;
        TransformNode[] minimapWallsTrans;
        TransformNode minimapBaseTrans;
        GeometryNode miniRealGroundnode;
        GeometryNode gear;
        TransformNode gearTrans;
        int objectCount = 0;
        Vector3 initGravityDir = new Vector3(0, 0, -9.8f);

        //delete
        //GeometryNode[] wallNode;
        //TransformNode[] wallTransNode;
        //Material[] wallMaterial;

        //menu
        MarkerNode hand1MarkerNode, hand2MarkerNode, hand3MarkerNode;
        GeometryNode menuPointerNode;

        TransformNode[] menuTransform;
        GeometryNode[] menuItems;
        bool confirmSelection = false;
        int selectedLeaf = -1;
        int currentMenuHeirarchy = 0;
        int confirmstep = 0;
        TransformNode menuHighlighterTrans;
        GeometryNode menuHighlighter;
        Texture2D createMenuTexture;

        //selection and manipulation
        int currentManipulation = 0; //1 for translate, 2 for rotate, 3 for scale
        int selectedObject = -1;
        float distance;

        GeometryNode groundPointerNode;
        GeometryNode objectHighlighter;
        TransformNode objectHighlighterTrans;

        Matrix prevWorldTransform;
        public TransformNode rootTransNode1, rootTransNode2, travelFirstTransNode, travelSecTransNode, travelThirdTransNode, boxTransNode2, realgroundTransNode;
        Vector3 cameraPos = new Vector3(0, 0, 0), groundRefPos, cameraDelt, carPos, oldCameraPos;
        float travelTranslationY = 0;
        double miniUpdateAngle, miniInitAngle, miniMapRotateDegree = 0, groundRefAngle, speedUpdateAngle, speedInitAngle;

        //delete
        //int wallindex = 0;
        //int iniWallSize = 10;
        //delete end

        bool debug = true;
        bool travelMode = false;
        bool miniMapUpdateOnce = false, setOnce = false, carPosAlreadySet = false, speedControlUpdateOnce = false, collision1 = false, collision2 = false;
        bool boxNodeVisible = true;
        //Sam
        double deltAngle, speedTransChange;
        WallCreator wallCreator;
        RaceCar racecar;

        //You on the minimap
        TransformNode youTrans1, youTrans2, youTrans;
        Vector3 oldPos, newPos;
        int counter = 0;
        float scaleObjRatio = 5.0f;
        TransformNode miniMapTransNode = new TransformNode();
        TransformNode miniRealgroundTransNode;

        //Model
        GeometryNode shipNode = new GeometryNode("ship");
        TransformNode shipTransNode = new TransformNode();

        public Game1()
        {
            graphics = new GraphicsDeviceManager(this);
            Content.RootDirectory = "Content";
        }

        /// <summary>
        /// Allows the game to perform any initialization it needs to before starting to run.
        /// This is where it can query for any required services and load any non-graphic
        /// related content.  Calling base.Initialize will enumerate through any components
        /// and initialize them as well.
        /// </summary>
        protected override void Initialize()
        {
            // Initialize the GoblinXNA framework
            State.InitGoblin(graphics, Content, "");

            // Initialize the scene graph
            scene = new Scene(this);

            // Use the newton physics engine to perform collision detection
            scene.PhysicsEngine = new NewtonPhysics();
            scene.PhysicsEngine.GravityDirection = initGravityDir;
            //scene.PhysicsEngine.GravityDirection = new Vector3(0, 0, 0f);   

            // For some reason, it causes memory conflict when it attempts to update the
            // marker transformation in the multi-threaded code, so if you're using ARTag
            // then you should not enable the marker tracking thread
#if !USE_ARTAG
            State.ThreadOption = (ushort)ThreadOptions.MarkerTracking;
#endif

            // Set up optical marker tracking
            // Note that we don't create our own camera when we use optical marker
            // tracking. It'll be created automatically
            SetupMarkerTracking();

            // Set up the lights used in the scene
            CreateLights();

            //ALL MARKERS
            createAllMarkers();
            // Create the ground that represents the physical ground marker array
            CreateGround();

            //Sam
            CreateNewtonWorld();
            CreateVisualGround();
            createMiniVisualGround();
            wallCreator.addRealMiniMapNode(miniRealGroundnode);

            // Create 3D objects
            CreateObjects();

            createCarWallCollision();

            // Use per pixel lighting for better quality (If you using non NVidia graphics card,
            // setting this to true may reduce the performance significantly)
            scene.PreferPerPixelLighting = false;

            // Enable shadow mapping
            // NOTE: In order to use shadow mapping, you will need to add 'PostScreenShadowBlur.fx'
            // and 'ShadowMap.fx' shader files as well as 'ShadowDistanceFadeoutMap.dds' texture file
            // to your 'Content' directory
            scene.EnableShadowMapping = false;

            // Show Frames-Per-Second on the screen for debugging
            State.ShowFPS = true;
            State.ShowNotifications = true;

            // Add a keyboard press handler for user input
            KeyboardInput.Instance.KeyPressEvent += new HandleKeyPress(KeyPressHandler);


            base.Initialize();
        }

        private void CreateLights()
        {
            // Create a directional light source
            LightSource lightSource = new LightSource();
            lightSource.Direction = new Vector3(1, -1, -1);
            lightSource.Diffuse = Color.White.ToVector4();
            lightSource.Specular = new Vector4(0.6f, 0.6f, 0.6f, 1);

            // Create a light node to hold the light source
            LightNode lightNode = new LightNode();
            lightNode.LightSource = lightSource;

            scene.RootNode.AddChild(lightNode);
        }

        private void SetupMarkerTracking()
        {
            // Create our video capture device that uses DirectShow library. Note that 
            // the combinations of resolution and frame rate that are allowed depend on 
            // the particular video capture device. Thus, setting incorrect resolution 
            // and frame rate values may cause exceptions or simply be ignored, depending 
            // on the device driver.  The values set here will work for a Microsoft VX 6000, 
            // and many other webcams.
            DirectShowCapture captureDevice = new DirectShowCapture();
            captureDevice.InitVideoCapture(0, FrameRate._30Hz, Resolution._640x480,
                ImageFormat.R8G8B8_24, false);

            // Add this video capture device to the scene so that it can be used for
            // the marker tracker
            scene.AddVideoCaptureDevice(captureDevice);

            IMarkerTracker tracker = null;

#if USE_ARTAG
            // Create an optical marker tracker that uses ARTag library
            tracker = new ARTagTracker();
            // Set the configuration file to look for the marker specifications
            tracker.InitTracker(638.052f, 633.673f, captureDevice.Width,
                captureDevice.Height, false, "ARTag.cf");
#else
            // Create an optical marker tracker that uses ALVAR library
            tracker = new ALVARMarkerTracker();
            ((ALVARMarkerTracker)tracker).MaxMarkerError = 0.02f;
            tracker.InitTracker(captureDevice.Width, captureDevice.Height, "calib.xml", 9.0);
#endif

            // Set the marker tracker to use for our scene
            scene.MarkerTracker = tracker;

            // Display the camera image in the background. Note that this parameter should
            // be set after adding at least one video capture device to the Scene class.
            scene.ShowCameraImage = true;
        }

        private void CreateGround()
        {
            //groundNode = new GeometryNode("Ground");
            //groundNode.Model = new Box(10000f, 100000f, 0.1f);

            //// Set this ground model to act as an occluder so that it appears transparent
            //groundNode.IsOccluder = true;
            //groundNode.AddToPhysicsEngine = true;
            //groundNode.Physics.Interactable = false;
            //groundNode.Physics.Collidable = false;

            //// Make the ground model to receive shadow casted by other objects with
            //// CastShadows set to true
            //groundNode.Model.ReceiveShadows = true;

            //Material groundMaterial = new Material();
            //groundMaterial.Diffuse = Color.Red.ToVector4();
            //groundMaterial.Specular = Color.White.ToVector4();
            //groundMaterial.SpecularPower = 20;

            //groundNode.Material = groundMaterial;




            groundNode = new GeometryNode("Ground");
            groundNode.Model = new Box(10000f, 13000f, 0.1f);

            // Set this ground model to act as an occluder so that it appears transparent
            groundNode.IsOccluder = true;

            // Make the ground model to receive shadow casted by other objects with
            // CastShadows set to true
            groundNode.Model.ReceiveShadows = true;

            Material groundMaterial = new Material();
            groundMaterial.Diffuse = Color.Gray.ToVector4();
            groundMaterial.Specular = Color.White.ToVector4();
            groundMaterial.SpecularPower = 20;

            groundNode.Material = groundMaterial;

            groundNode.Physics.Interactable = false;
            groundNode.Physics.Collidable = true;
            groundNode.Physics.Shape = ShapeType.Box;
            groundNode.Physics.Mass = 10f;
            groundNode.Physics.ApplyGravity = false;
            groundNode.AddToPhysicsEngine = true;
            rootTransNode1.AddChild(groundNode);

        }

        private void CreateObjects()
        {
            //ModelLoader loader = new ModelLoader();
            //Model shipModel = (Model)loader.Load("", "create");

            //// Create a geometry node of a loaded ship model
            ////GeometryNode shipNode = new GeometryNode("Ship");
            //shipNode.Model = shipModel;
            //// This ship model has material definitions in the model file, so instead
            //// of creating a material node for this ship model, we simply use its internal materials
            ////((Model)shipNode.Model).UseInternalMaterials = true;
            //Material testMat = new Material();
            //testMat.Diffuse = Color.Red.ToVector4();
            //testMat.Specular = Color.White.ToVector4();
            //shipNode.Material = testMat;

            //((Model)shipNode.Model).Enabled = true;
            //hand1MarkerNode.AddChild(shipTransNode);
            //shipTransNode.AddChild(shipNode);

            //shipTransNode.Scale = new Vector3(20, 20, 20);

            //You on the minimap
            GeometryNode you = new GeometryNode("You");
            you.Model = new Cylinder(1.5f, 0, 4f, 20);
            Material youOnMiniMap = new Material();
            youOnMiniMap.Diffuse = Color.Red.ToVector4();
            youOnMiniMap.Specular = Color.White.ToVector4();
            you.IgnoreDepth = true;
            you.Material = youOnMiniMap;

            youTrans = new TransformNode();
            youTrans1 = new TransformNode();
            youTrans2 = new TransformNode();
            youTrans1.Translation = new Vector3(30 * 0.3f, -30 * 0.3f, 0);
            youTrans.Rotation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, -(float)System.Math.PI / 2);
            youTrans.AddChild(you);
            youTrans1.AddChild(youTrans);
            youTrans2.AddChild(youTrans1);
            miniMapMarkerNode.AddChild(youTrans2);

            ////delete
            //wallNode = new GeometryNode[100];
            //wallMaterial = new Material[100];
            sceneObjects = new GeometryNode[100];
            sceneObjectsTrans = new TransformNode[100];
            minimapObjects = new GeometryNode[100];
            minimapObjectsTrans = new TransformNode[100];
            minimapWalls = new GeometryNode[100];
            minimapWallsTrans = new TransformNode[100];

            //SelectionPointer
            initGroundPointer();
            initObjectHighlighter();

            //MENU
            //Initialize pointer first. Then initialize menu so that collision works.
            initMenuPointer();
            initHandMenu();


            racecar = new RaceCar(wallCreator, scene, groundMarkerNode, realgroundNode, groundNode, 30, -30);

            initMap();
            initMiniMap();

            gear = new GeometryNode();
            gear.Model = new Sphere(3, 20, 20);
            gear.IgnoreDepth = true;
            Material gearMaterial = new Material();
            gearMaterial.Diffuse = new Vector4(Color.Yellow.ToVector3(), 1.0f);
            gear.Material = gearMaterial;
            gearTrans = new TransformNode();
            speedMarkerNode.AddChild(gearTrans);
            gearTrans.Translation = new Vector3(4, -4, 4);
            gearTrans.AddChild(gear);
        }
        private void initObjectHighlighter()
        {
            objectHighlighterTrans = new TransformNode();
            objectHighlighter = new GeometryNode();
            objectHighlighter.Model = new Sphere(8, 20, 20);
            Material highlighterMaterial = new Material();
            highlighterMaterial.Diffuse = new Vector4(Color.White.ToVector3(), 0.3f);
            objectHighlighter.Material = highlighterMaterial;
            objectHighlighterTrans.Translation = new Vector3(-1000, -1000, -1000);
            realgroundNode.AddChild(objectHighlighterTrans);
            objectHighlighterTrans.AddChild(objectHighlighter);
        }
        private void initGroundPointer()
        {
            groundPointerNode = new GeometryNode("Pointer");
            groundPointerNode.Model = new Sphere(5, 20, 20);
            Material pointerMaterial = new Material();
            pointerMaterial.Diffuse = new Vector4(Color.Gold.ToVector3(), 0.3f);
            pointerMaterial.Emissive = Color.Gray.ToVector4();
            pointerMaterial.Specular = Color.White.ToVector4();
            groundPointerNode.Material = pointerMaterial;
            groundPointerNode.AddToPhysicsEngine = true;
            groundPointerNode.Physics.Shape = ShapeType.Sphere;

        }
        private void createAllMarkers()
        {
            //Ground Marker
            groundMarkerNode = new MarkerNode(scene.MarkerTracker, "21-60_ALVARArray_GroundMarker.txt");
            scene.RootNode.AddChild(groundMarkerNode);

            //Hand Markers
            hand1MarkerNode = new MarkerNode(scene.MarkerTracker, "ALVAR_Hand1.txt"); //Menu
            hand2MarkerNode = new MarkerNode(scene.MarkerTracker, "ALVAR_Hand2.txt"); //Start/Stop
            hand3MarkerNode = new MarkerNode(scene.MarkerTracker, "ALVAR_Hand3.txt"); //pointer

            scene.RootNode.AddChild(hand1MarkerNode); //Menu
            scene.RootNode.AddChild(hand2MarkerNode); //Start/Stop
            scene.RootNode.AddChild(hand3MarkerNode); //Pointer
            //Create the marker node for the minimap
            miniMapMarkerNode = new MarkerNode(scene.MarkerTracker, "1-4_ALVAR_MiniMap.txt");
            scene.RootNode.AddChild(miniMapMarkerNode);
            minimapBaseTrans = new TransformNode();
            minimapBaseTrans.Scale = new Vector3(0.1f, 0.1f, 0.1f);
            miniMapMarkerNode.AddChild(minimapBaseTrans);

            groundReferenceMarkerNode = new MarkerNode(scene.MarkerTracker, "11-12_ALVAR_ground_reference.txt");
            scene.RootNode.AddChild(groundReferenceMarkerNode);

            speedMarkerNode = new MarkerNode(scene.MarkerTracker, "0_ALVAR_speedControl.txt");
            scene.RootNode.AddChild(speedMarkerNode);

            rootTransNode1 = new TransformNode("rootTransNode1");
            rootTransNode2 = new TransformNode("rootTransNode2");

            groundMarkerNode.AddChild(rootTransNode2);
            rootTransNode2.AddChild(rootTransNode1);
        }


        /// <summary>
        /// Initialises the pointer on the right hand which used for selecting menu items and direct manipulation
        /// Create a pointerHead node to signify the head of pointer.
        /// </summary>
        private void initMenuPointer()
        {

            menuPointerNode = new GeometryNode("Pointer");
            menuPointerNode.Model = new Sphere(5, 20, 20);
            Material pointerMaterial = new Material();
            //Color c = new Color();
            pointerMaterial.Diffuse = new Vector4(Color.Gold.ToVector3(), 0.3f);
            pointerMaterial.Emissive = Color.Gold.ToVector4();
            //pointerMaterial.
            pointerMaterial.Specular = Color.White.ToVector4();
            menuPointerNode.Material = pointerMaterial;
            menuPointerNode.AddToPhysicsEngine = true;
            menuPointerNode.Physics.Shape = ShapeType.Sphere;
        }





        /// <summary>
        /// Initialize all menu item geometry nodes and set up appropriate collision pairs with pointer.
        /// </summary>
        private void initHandMenu()
        {
            //Create all heirarchies of menu. Add only the first heirarchy to the scene.
            menuTransform = new TransformNode[5];
            //wrist
            menuTransform[0] = new TransformNode();
            menuTransform[0].Translation = new Vector3(0, -40, 15);

            //Index finger
            menuTransform[1] = new TransformNode();
            menuTransform[1].Translation = new Vector3(-17, 30, 15);

            //Middle finger
            menuTransform[2] = new TransformNode();
            menuTransform[2].Translation = new Vector3(0, 35, 15);

            //Ring finger
            menuTransform[3] = new TransformNode();
            menuTransform[3].Translation = new Vector3(17, 30, 15);

            //Pinky
            menuTransform[4] = new TransformNode();
            menuTransform[4].Translation = new Vector3(30, 20, 15);

            hand1MarkerNode.AddChild(menuTransform[0]);
            hand1MarkerNode.AddChild(menuTransform[1]);
            hand1MarkerNode.AddChild(menuTransform[2]);
            hand1MarkerNode.AddChild(menuTransform[3]);
            hand1MarkerNode.AddChild(menuTransform[4]);

            menuHighlighterTrans = new TransformNode();
            menuHighlighter = new GeometryNode();
            menuHighlighter.Model = new Sphere(8, 20, 20);
            Material highlighterMaterial = new Material();
            highlighterMaterial.Diffuse = new Vector4(Color.White.ToVector3(), 0.3f);
            menuHighlighter.Material = highlighterMaterial;
            hand1MarkerNode.AddChild(menuHighlighterTrans);
            menuHighlighterTrans.AddChild(menuHighlighter);
            menuHighlighterTrans.Translation = new Vector3(-1000, -1000, -1000);
            // The number of elements in the menuItems array signifies exactly how many nodes are there including ALL levels of heirarchy.
            menuItems = new GeometryNode[20];

            //Following code initializes all geometry nodes necessary for all heirarchies of the menu.
            //Sets up their collision pairs with the "pointer" (hand3), and the callbacks involved.


            menuItems[MENU.CREATE] = new GeometryNode("CREATE");
            menuItems[MENU.CREATE].Model = new Box(8, 8, 1);
            Material itemMaterial1 = new Material();
            itemMaterial1.Diffuse = Color.Blue.ToVector4();
            itemMaterial1.Specular = Color.White.ToVector4();
            //createMenuTexture = new Texture2D(graphics.GraphicsDevice, 20, 20);

            menuItems[MENU.CREATE].AddToPhysicsEngine = true;
            menuItems[MENU.CREATE].Physics.Shape = ShapeType.Box;
            NewtonPhysics.CollisionPair pair1 = new NewtonPhysics.CollisionPair(menuItems[MENU.CREATE].Physics, menuPointerNode.Physics);
            ((NewtonPhysics)scene.PhysicsEngine).AddCollisionCallback(pair1, createSelected);


            menuItems[MENU.CREATE].Material = itemMaterial1;

            menuItems[MENU.MANIPULATE] = new GeometryNode("MANIPULATE");
            menuItems[MENU.MANIPULATE].Model = new Box(8, 8, 1);
            Material itemMaterial2 = new Material();
            itemMaterial2.Diffuse = Color.Red.ToVector4();
            itemMaterial2.Specular = Color.White.ToVector4();
            menuItems[MENU.MANIPULATE].Material = itemMaterial2;
            menuItems[MENU.MANIPULATE].AddToPhysicsEngine = true;
            menuItems[MENU.MANIPULATE].Physics.Shape = ShapeType.Box;
            NewtonPhysics.CollisionPair pair2 = new NewtonPhysics.CollisionPair(menuItems[MENU.MANIPULATE].Physics, menuPointerNode.Physics);
            ((NewtonPhysics)scene.PhysicsEngine).AddCollisionCallback(pair2, manipulateSelected);

            menuItems[MENU.SWITCHMODE] = new GeometryNode("SWITCHMODE");
            menuItems[MENU.SWITCHMODE].Model = new Box(8, 8, 1);
            Material itemMaterial3 = new Material();
            itemMaterial3.Diffuse = Color.Green.ToVector4();
            itemMaterial3.Specular = Color.White.ToVector4();
            menuItems[MENU.SWITCHMODE].Material = itemMaterial3;
            menuItems[MENU.SWITCHMODE].AddToPhysicsEngine = true;
            menuItems[MENU.SWITCHMODE].Physics.Shape = ShapeType.Box;
            NewtonPhysics.CollisionPair pair3 = new NewtonPhysics.CollisionPair(menuItems[MENU.SWITCHMODE].Physics, menuPointerNode.Physics);
            ((NewtonPhysics)scene.PhysicsEngine).AddCollisionCallback(pair3, switchmodeSelected);

            menuItems[MENU.BACK] = new GeometryNode("BACK");
            menuItems[MENU.BACK].Model = new Box(8, 8, 1);
            Material itemMaterial4 = new Material();
            itemMaterial4.Diffuse = Color.Green.ToVector4();
            itemMaterial4.Specular = Color.White.ToVector4();
            menuItems[MENU.BACK].Material = itemMaterial4;
            menuItems[MENU.BACK].AddToPhysicsEngine = true;
            menuItems[MENU.BACK].Physics.Shape = ShapeType.Box;
            NewtonPhysics.CollisionPair pair4 = new NewtonPhysics.CollisionPair(menuItems[MENU.BACK].Physics, menuPointerNode.Physics);
            ((NewtonPhysics)scene.PhysicsEngine).AddCollisionCallback(pair4, backSelected);

            menuItems[MENU.CUBE] = new GeometryNode("CUBE");
            menuItems[MENU.CUBE].Model = new Box(5, 5, 5);
            Material itemMaterial5 = new Material();
            itemMaterial5.Diffuse = Color.Blue.ToVector4();
            itemMaterial5.Specular = Color.White.ToVector4();
            menuItems[MENU.CUBE].Material = itemMaterial5;
            menuItems[MENU.CUBE].AddToPhysicsEngine = true;
            menuItems[MENU.CUBE].Physics.Shape = ShapeType.Box;
            NewtonPhysics.CollisionPair pair5 = new NewtonPhysics.CollisionPair(menuItems[MENU.CUBE].Physics, menuPointerNode.Physics);
            ((NewtonPhysics)scene.PhysicsEngine).AddCollisionCallback(pair5, cubeSelected);

            menuItems[MENU.CYLINDER] = new GeometryNode("SPHERE");
            menuItems[MENU.CYLINDER].Model = new Cylinder(2.5f, 2.5f, 10, 10);
            Material itemMaterial6 = new Material();
            itemMaterial6.Diffuse = Color.Red.ToVector4();
            itemMaterial6.Specular = Color.White.ToVector4();
            menuItems[MENU.CYLINDER].Material = itemMaterial6;
            menuItems[MENU.CYLINDER].AddToPhysicsEngine = true;
            menuItems[MENU.CYLINDER].Physics.Shape = ShapeType.Sphere;
            NewtonPhysics.CollisionPair pair6 = new NewtonPhysics.CollisionPair(menuItems[MENU.CYLINDER].Physics, menuPointerNode.Physics);
            ((NewtonPhysics)scene.PhysicsEngine).AddCollisionCallback(pair6, sphereSelected);

            menuItems[MENU.TRANSLATE] = new GeometryNode("TRANSLATE");
            menuItems[MENU.TRANSLATE].Model = new Box(8, 8, 1);
            Material itemMaterial7 = new Material();
            itemMaterial7.Diffuse = Color.Blue.ToVector4();
            itemMaterial7.Specular = Color.White.ToVector4();
            menuItems[MENU.TRANSLATE].Material = itemMaterial7;
            menuItems[MENU.TRANSLATE].AddToPhysicsEngine = true;
            menuItems[MENU.TRANSLATE].Physics.Shape = ShapeType.Box;
            NewtonPhysics.CollisionPair pair7 = new NewtonPhysics.CollisionPair(menuItems[MENU.TRANSLATE].Physics, menuPointerNode.Physics);
            ((NewtonPhysics)scene.PhysicsEngine).AddCollisionCallback(pair7, translateSelected);

            menuItems[MENU.ROTATE] = new GeometryNode("ROTATE");
            menuItems[MENU.ROTATE].Model = new Box(8, 8, 1);
            Material itemMaterial8 = new Material();
            itemMaterial8.Diffuse = Color.Red.ToVector4();
            itemMaterial8.Specular = Color.White.ToVector4();
            menuItems[MENU.ROTATE].Material = itemMaterial8;
            menuItems[MENU.ROTATE].AddToPhysicsEngine = true;
            menuItems[MENU.ROTATE].Physics.Shape = ShapeType.Box;
            NewtonPhysics.CollisionPair pair8 = new NewtonPhysics.CollisionPair(menuItems[MENU.ROTATE].Physics, menuPointerNode.Physics);
            ((NewtonPhysics)scene.PhysicsEngine).AddCollisionCallback(pair8, rotateSelected);

            menuItems[MENU.SCALE] = new GeometryNode("SCALE");
            menuItems[MENU.SCALE].Model = new Box(8, 8, 1);
            Material itemMaterial9 = new Material();
            itemMaterial9.Diffuse = Color.Green.ToVector4();
            itemMaterial9.Specular = Color.White.ToVector4();
            menuItems[MENU.SCALE].Material = itemMaterial9;
            menuItems[MENU.SCALE].AddToPhysicsEngine = true;
            menuItems[MENU.SCALE].Physics.Shape = ShapeType.Box;
            NewtonPhysics.CollisionPair pair9 = new NewtonPhysics.CollisionPair(menuItems[MENU.SCALE].Physics, menuPointerNode.Physics);
            ((NewtonPhysics)scene.PhysicsEngine).AddCollisionCallback(pair9, scaleSelected);

            menuItems[MENU.XAXIS] = new GeometryNode("XAXIS");
            menuItems[MENU.XAXIS].Model = new Box(8, 2, 2);
            Material itemMaterial10 = new Material();
            itemMaterial10.Diffuse = Color.Blue.ToVector4();
            itemMaterial10.Specular = Color.White.ToVector4();
            menuItems[MENU.XAXIS].Material = itemMaterial10;
            menuItems[MENU.XAXIS].AddToPhysicsEngine = true;
            menuItems[MENU.XAXIS].Physics.Shape = ShapeType.Box;
            NewtonPhysics.CollisionPair pair10 = new NewtonPhysics.CollisionPair(menuItems[MENU.XAXIS].Physics, menuPointerNode.Physics);
            ((NewtonPhysics)scene.PhysicsEngine).AddCollisionCallback(pair10, xaxisSelected);

            menuItems[MENU.YAXIS] = new GeometryNode("YAXIS");
            menuItems[MENU.YAXIS].Model = new Box(2, 8, 2);
            Material itemMaterial11 = new Material();
            itemMaterial11.Diffuse = Color.Red.ToVector4();
            itemMaterial11.Specular = Color.White.ToVector4();
            menuItems[MENU.YAXIS].Material = itemMaterial11;
            menuItems[MENU.YAXIS].AddToPhysicsEngine = true;
            menuItems[MENU.YAXIS].Physics.Shape = ShapeType.Box;
            NewtonPhysics.CollisionPair pair11 = new NewtonPhysics.CollisionPair(menuItems[MENU.YAXIS].Physics, menuPointerNode.Physics);
            ((NewtonPhysics)scene.PhysicsEngine).AddCollisionCallback(pair11, yaxisSelected);

            menuItems[MENU.ZAXIS] = new GeometryNode("ZAXIS");
            menuItems[MENU.ZAXIS].Model = new Box(2, 2, 8);
            Material itemMaterial12 = new Material();
            itemMaterial12.Diffuse = Color.Green.ToVector4();
            itemMaterial12.Specular = Color.White.ToVector4();
            menuItems[MENU.ZAXIS].Material = itemMaterial12;
            menuItems[MENU.ZAXIS].AddToPhysicsEngine = true;
            menuItems[MENU.ZAXIS].Physics.Shape = ShapeType.Box;
            NewtonPhysics.CollisionPair pair12 = new NewtonPhysics.CollisionPair(menuItems[MENU.ZAXIS].Physics, menuPointerNode.Physics);
            ((NewtonPhysics)scene.PhysicsEngine).AddCollisionCallback(pair12, zaxisSelected);

            menuItems[MENU.WALL] = new GeometryNode("CUBE");
            menuItems[MENU.WALL].Model = new Box(10, 15, 5);
            Material itemMaterial13 = new Material();
            itemMaterial13.Diffuse = Color.Green.ToVector4();
            itemMaterial13.Specular = Color.White.ToVector4();
            menuItems[MENU.WALL].Material = itemMaterial13;
            menuItems[MENU.WALL].AddToPhysicsEngine = true;
            menuItems[MENU.WALL].Physics.Shape = ShapeType.Box;
            NewtonPhysics.CollisionPair pair13 = new NewtonPhysics.CollisionPair(menuItems[MENU.WALL].Physics, menuPointerNode.Physics);
            ((NewtonPhysics)scene.PhysicsEngine).AddCollisionCallback(pair13, wallSelected);

            //After initializing all menu nodes, load the root heirarchy of the menu.
            loadMainMenu();
        }

        /// <summary>
        /// Loads the root level menu
        /// </summary>
        private void loadMainMenu()
        {
            try
            {
                menuTransform[1].AddChild(menuItems[MENU.CREATE]);
                menuTransform[2].AddChild(menuItems[MENU.MANIPULATE]);
                menuTransform[3].AddChild(menuItems[MENU.SWITCHMODE]);

                currentMenuHeirarchy = MENU.MAIN;
                if (selectedObject >= 0)
                {
                    menuItems[MENU.MANIPULATE].Material.Diffuse = Color.Red.ToVector4();
                }
                else
                {
                    menuItems[MENU.MANIPULATE].Material.Diffuse = Color.Gray.ToVector4();
                }
            }
            catch (Exception e)
            {
            }
        }
        /// <summary>
        /// Unloads root level menu
        /// </summary>
        private void unloadMainMenu()
        {
            try
            {
                menuTransform[1].RemoveChild(menuItems[MENU.CREATE]);
                menuTransform[2].RemoveChild(menuItems[MENU.MANIPULATE]);
                menuTransform[3].RemoveChild(menuItems[MENU.SWITCHMODE]);
            }
            catch (Exception e)
            {
            }
        }
        /// <summary>
        /// Loads the object creation menu - all objects that can be created
        /// </summary>
        private void loadObjectsMenu()
        {
            try
            {
                menuTransform[0].AddChild(menuItems[MENU.BACK]);
                menuTransform[1].AddChild(menuItems[MENU.CUBE]);
                menuTransform[2].AddChild(menuItems[MENU.CYLINDER]);
                menuTransform[3].AddChild(menuItems[MENU.WALL]);
                currentMenuHeirarchy = MENU.CREATE;
            }
            catch
            {
            }
        }
        /// <summary>
        /// Unloads the object creation menu - all objects that can be created
        /// </summary>
        private void unloadObjectsMenu()
        {
            try
            {
                menuTransform[0].RemoveChild(menuItems[MENU.BACK]);
                menuTransform[1].RemoveChild(menuItems[MENU.CUBE]);
                menuTransform[2].RemoveChild(menuItems[MENU.CYLINDER]);
                menuTransform[3].RemoveChild(menuItems[MENU.WALL]);
            }
            catch
            {
            }
        }
        /// <summary>
        /// Loads the manipulation menu - Scale, rotate, translate
        /// </summary>
        private void loadManipulationMenu()
        {
            try
            {
                menuTransform[0].AddChild(menuItems[MENU.BACK]);
                menuTransform[1].AddChild(menuItems[MENU.TRANSLATE]);
                menuTransform[2].AddChild(menuItems[MENU.ROTATE]);
                menuTransform[3].AddChild(menuItems[MENU.SCALE]);
                currentMenuHeirarchy = MENU.MANIPULATE;
            }
            catch (Exception e)
            {
            }
        }
        /// <summary>
        /// Unloads the manipulation menu
        /// </summary>
        private void unloadManipulationMenu()
        {
            try
            {
                menuTransform[0].RemoveChild(menuItems[MENU.BACK]);
                menuTransform[1].RemoveChild(menuItems[MENU.TRANSLATE]);
                menuTransform[2].RemoveChild(menuItems[MENU.ROTATE]);
                menuTransform[3].RemoveChild(menuItems[MENU.SCALE]);
            }
            catch (Exception e)
            {
            }
        }
        /// <summary>
        /// Loads the Axes Menu
        /// </summary>
        private void loadAxesMenu()
        {
            try
            {
                //         menuTransform[0].AddChild(menuItems[MENU.BACK]);
                menuTransform[1].AddChild(menuItems[MENU.XAXIS]);
                menuTransform[2].AddChild(menuItems[MENU.YAXIS]);
                menuTransform[3].AddChild(menuItems[MENU.ZAXIS]);
                currentMenuHeirarchy = MENU.AXES;
            }
            catch
            {
            }
        }
        /// <summary>
        /// Unloads the Axes Menu
        /// </summary>
        private void unloadAxesMenu()
        {
            try
            {
                menuTransform[0].RemoveChild(menuItems[MENU.BACK]);
                menuTransform[1].RemoveChild(menuItems[MENU.XAXIS]);
                menuTransform[2].RemoveChild(menuItems[MENU.YAXIS]);
                menuTransform[3].RemoveChild(menuItems[MENU.ZAXIS]);
            }
            catch (Exception e)
            {
            }
        }

        /// <summary>
        /// Once user selects the Create Node, go one level deeper into heirarchy - load the Object creation menu
        /// </summary>
        /// <param name="pair"></param>
        private void createSelected(NewtonPhysics.CollisionPair pair)
        {
            selectedLeaf = MENU.CREATE;
            confirmSelection = false;
            menuHighlighterTrans.Translation = menuTransform[1].Translation;
            //wait for confirmation event
        }
        /// <summary>
        /// Once user selects the Manipulate Node, go one level deeper into heirarchy - load the Manipulation menu
        /// </summary>
        /// <param name="pair"></param>
        private void manipulateSelected(NewtonPhysics.CollisionPair pair)
        {
            if (selectedObject >= 0)
            {
                selectedLeaf = MENU.MANIPULATE;
                confirmSelection = false;

                menuHighlighterTrans.Translation = menuTransform[2].Translation;
            }
            //wait for confirmation event
        }
        /// <summary>
        /// Once user selects the Translate Node, go one level deeper into heirarchy - load the axes menu
        /// </summary>
        /// <param name="pair"></param>
        private void translateSelected(NewtonPhysics.CollisionPair pair)
        {
            selectedLeaf = MENU.TRANSLATE;
            confirmSelection = false;
            menuHighlighterTrans.Translation = menuTransform[1].Translation;
            //wait for confirmation event
        }

        /// <summary>
        /// Once user selects the Rotate Node, go one level deeper into heirarchy - load the axes menu
        /// </summary>
        /// <param name="pair"></param>
        private void rotateSelected(NewtonPhysics.CollisionPair pair)
        {
            selectedLeaf = MENU.ROTATE;
            confirmSelection = false;
            menuHighlighterTrans.Translation = menuTransform[2].Translation;
            //wait for confirmation event
        }

        /// <summary>
        /// Once user selects the Scale Node, go one level deeper into heirarchy - load the axes menu
        /// </summary>
        /// <param name="pair"></param>
        private void scaleSelected(NewtonPhysics.CollisionPair pair)
        {
            selectedLeaf = MENU.SCALE;
            confirmSelection = false;
            menuHighlighterTrans.Translation = menuTransform[3].Translation;
            //wait for confirmation event
        }

        private void xaxisSelected(NewtonPhysics.CollisionPair pair)
        {
            selectedLeaf = MENU.XAXIS;
            confirmSelection = false;
            menuHighlighterTrans.Translation = menuTransform[1].Translation;
            //wait for confirmation event
        }
        private void yaxisSelected(NewtonPhysics.CollisionPair pair)
        {
            selectedLeaf = MENU.YAXIS;
            confirmSelection = false;
            menuHighlighterTrans.Translation = menuTransform[2].Translation;
            //wait for confirmation event
        }
        private void zaxisSelected(NewtonPhysics.CollisionPair pair)
        {
            selectedLeaf = MENU.ZAXIS;
            confirmSelection = false;
            menuHighlighterTrans.Translation = menuTransform[3].Translation;
            //wait for confirmation event
        }
        private void switchmodeSelected(NewtonPhysics.CollisionPair pair)
        {
            selectedLeaf = MENU.SWITCHMODE;
            confirmSelection = false;
            menuHighlighterTrans.Translation = menuTransform[3].Translation;
            //wait for confirmation event
        }
        private void cubeSelected(NewtonPhysics.CollisionPair pair)
        {
            selectedLeaf = MENU.CUBE;
            confirmSelection = false;
            menuHighlighterTrans.Translation = menuTransform[1].Translation;
            //wait for confirmation event
        }
        private void sphereSelected(NewtonPhysics.CollisionPair pair)
        {
            selectedLeaf = MENU.CYLINDER;
            confirmSelection = false;
            menuHighlighterTrans.Translation = menuTransform[2].Translation;
            //wait for confirmation
        }
        private void wallSelected(NewtonPhysics.CollisionPair pair)
        {
            selectedLeaf = MENU.WALL;
            confirmSelection = false;
            menuHighlighterTrans.Translation = menuTransform[3].Translation;
            //wait for confirmation
        }

        private void createWall()
        {
            //created
            Flags.createWallMode = true;
            wallCreator.wallindex++;

            wallCreator.wallNode[wallCreator.wallindex] = new GeometryNode("wall" + wallCreator.wallindex);
            wallCreator.wallNode[wallCreator.wallindex].Model = new Box(10, 5, 15);
            wallCreator.wallNode[wallCreator.wallindex].AddToPhysicsEngine = true;
            wallCreator.wallNode[wallCreator.wallindex].Physics.Shape = ShapeType.Box;
            Material wallMaterial = new Material();
            wallMaterial.Diffuse = Color.Aqua.ToVector4();
            wallMaterial.Specular = Color.White.ToVector4();
            wallCreator.wallNode[wallCreator.wallindex].Material = wallMaterial;
            wallCreator.wallNode[wallCreator.wallindex].AddToPhysicsEngine = true;
            wallCreator.wallNode[wallCreator.wallindex].Physics.Shape = ShapeType.Box;

            wallCreator.wallTransNode[wallCreator.wallindex] = new TransformNode();
            hand1MarkerNode.AddChild(wallCreator.wallTransNode[wallCreator.wallindex]);
            wallCreator.wallTransNode[wallCreator.wallindex].AddChild(wallCreator.wallNode[wallCreator.wallindex]);

            //create on minimap
            minimapWalls[wallCreator.wallindex] = new GeometryNode();
            minimapWalls[wallCreator.wallindex].Model = wallCreator.wallNode[wallCreator.wallindex].Model;
            minimapWalls[wallCreator.wallindex].Material = wallCreator.wallNode[wallCreator.wallindex].Material;
            minimapWallsTrans[wallCreator.wallindex] = new TransformNode();
        }

        private void createCube()
        {
            Flags.createMode = true;
            sceneObjects[objectCount] = new GeometryNode();
            sceneObjects[objectCount].Model = new Box(8, 8, 8);
            sceneObjects[objectCount].AddToPhysicsEngine = true;
            sceneObjects[objectCount].Physics.Shape = ShapeType.Box;
            Material cubeMaterial = new Material();
            cubeMaterial.Diffuse = Color.Aqua.ToVector4();
            cubeMaterial.Specular = Color.White.ToVector4();
            sceneObjects[objectCount].Material = cubeMaterial;

            sceneObjectsTrans[objectCount] = new TransformNode();
            hand1MarkerNode.AddChild(sceneObjectsTrans[objectCount]);
            sceneObjectsTrans[objectCount].AddChild(sceneObjects[objectCount]);
            minimapObjects[objectCount] = new GeometryNode();
            minimapObjects[objectCount].Model = sceneObjects[objectCount].Model;
            minimapObjects[objectCount].Material = sceneObjects[objectCount].Material;
            minimapObjectsTrans[objectCount] = new TransformNode();

            selectedObject = objectCount;

            objectCount++;
        }

        private void createSphere()
        {
            Flags.createMode = true;
            sceneObjects[objectCount] = new GeometryNode();
            sceneObjects[objectCount].Model = new Cylinder(4, 4, 10, 10);
            sceneObjects[objectCount].AddToPhysicsEngine = true;
            sceneObjects[objectCount].Physics.Shape = ShapeType.Cylinder;
            Material sphereMaterial = new Material();
            sphereMaterial.Diffuse = Color.Aqua.ToVector4();
            sphereMaterial.Specular = Color.White.ToVector4();
            sceneObjects[objectCount].Material = sphereMaterial;

            sceneObjectsTrans[objectCount] = new TransformNode();
            hand1MarkerNode.AddChild(sceneObjectsTrans[objectCount]);
            sceneObjectsTrans[objectCount].AddChild(sceneObjects[objectCount]);
            minimapObjects[objectCount] = new GeometryNode();
            minimapObjects[objectCount].Model = sceneObjects[objectCount].Model;
            minimapObjects[objectCount].Material = sceneObjects[objectCount].Material;
            minimapObjectsTrans[objectCount] = new TransformNode();

            selectedObject = objectCount;
            objectCount++;

        }

        private void backSelected()
        {
            switch (currentMenuHeirarchy)
            {
                case MENU.CREATE:
                    unloadObjectsMenu();
                    loadMainMenu();
                    break;
                case MENU.MANIPULATE:
                    unloadManipulationMenu();
                    loadMainMenu();
                    break;
                case MENU.AXES:
                    unloadAxesMenu();
                    System.Threading.Thread.Sleep(200);
                    loadManipulationMenu();
                    break;
                case 0:
                    break;
            }
        }
        private void backSelected(NewtonPhysics.CollisionPair pair)
        {
            menuHighlighterTrans.Translation = new Vector3(-1000, -1000, -1000);
            backSelected();
        }
        /// UnloadContent will be called once per game and is the place to unload
        /// all content.
        /// </summary>
        protected override void UnloadContent()
        {
            Content.Unload();
        }

        /// <summary>
        /// Allows the game to run logic such as updating the world,
        /// checking for collisions, gathering input, and playing audio.
        /// </summary>
        /// <param name="gameTime">Provides a snapshot of timing values.</param>
        protected override void Update(GameTime gameTime)
        {
            distance = System.Math.Abs(hand1MarkerNode.WorldTransformation.Translation.X - hand3MarkerNode.WorldTransformation.Translation.X);
            //travelFirstTransNode.Translation = new Vector3(0, travelTranslationY, 0);


            //Console.WriteLine("realground translation" + realgroundNode.WorldTransformation.Translation);
            //Console.WriteLine("ground translation" + groundNode.WorldTransformation.Translation);

            if ((Flags.createMode || Flags.manipulateMode) && hand3MarkerNode.MarkerFound)
            {
                //Notifier.AddMessage("say aa" + distance);		

                if (distance > 0)
                {
                    //Notifier.AddMessage(distance.ToString());
                    sceneObjectsTrans[selectedObject].Scale = new Vector3(distance * 0.1f);
                    sceneObjectsTrans[selectedObject].Translation = new Vector3(0, 0, 0);


                }
            }
            if (Flags.createWallMode && hand3MarkerNode.MarkerFound)
            {
                //Notifier.AddMessage("say aa" + distance);		

                if (distance > 0)
                {
                    //Notifier.AddMessage(distance.ToString());
                    wallCreator.wallTransNode[wallCreator.wallindex].Scale = new Vector3(distance * 0.1f, 1, distance * 0.1f);
                }
            }
            //else		
            // Flags.createMode = false;

            base.Update(gameTime);
        }

        /// <summary>
        /// This is called when the game should draw itself.
        /// </summary>
        /// <param name="gameTime">Provides a snapshot of timing values.</param>
        protected override void Draw(GameTime gameTime)
        {
            //MENU and POINTER
            if (hand1MarkerNode.MarkerFound)
            {
                if (hand3MarkerNode.MarkerFound)
                {
                    try
                    {
                        hand1MarkerNode.AddChild(menuPointerNode);
                    }
                    catch (Exception)
                    {
                        //Add child should happen only once. Next time the Add child function will give exception.
                        //Do nothing in that case
                    }
                    Vector3 shiftVector = new Vector3(4, -4, 4);
                    Matrix mat = Matrix.CreateTranslation(shiftVector) *
                          hand3MarkerNode.WorldTransformation *
                          Matrix.Invert(hand1MarkerNode.WorldTransformation);
                    ((NewtonPhysics)scene.PhysicsEngine).SetTransform(menuPointerNode.Physics, mat);
                }
                else
                {
                    try
                    {
                        hand1MarkerNode.RemoveChild(menuPointerNode);
                    }
                    catch (Exception)
                    {
                        //Remove child should happen only once. Next time the remove child function will give exception.
                        //Do nothing in that case
                    }
                }
            }

            //Step 1 of confirm event.
            if (hand3MarkerNode.MarkerFound)
            {
                confirmstep = 1;

            }
            //Step 2 of Confirmation Event
            if (hand2MarkerNode.MarkerFound && confirmstep == 1 && confirmSelection == false && hand1MarkerNode.MarkerFound)
            {
                menuHighlighterTrans.WorldTransformation = Matrix.CreateTranslation(new Vector3(-1000, -1000, -1000));

                if (selectedLeaf != -1)
                {
                    switch (selectedLeaf)
                    {
                        case MENU.SWITCHMODE:
                            if (travelMode)
                                travelMode = false;
                            else
                                travelMode = true;
                            confirmSelection = true;
                            confirmstep = 2;
                            break;
                        case MENU.CUBE:
                            confirmSelection = true;
                            unloadObjectsMenu();
                            createCube();
                            confirmstep = 2;
                            break;
                        case MENU.CYLINDER:
                            confirmSelection = true;
                            unloadObjectsMenu();
                            createSphere();
                            confirmstep = 2;
                            break;
                        case MENU.WALL:
                            confirmSelection = true;
                            unloadObjectsMenu();
                            createWall();
                            confirmstep = 2;
                            break;
                        case MENU.CREATE:
                            confirmSelection = true;
                            unloadMainMenu();
                            loadObjectsMenu();
                            confirmstep = 2;
                            break;
                        case MENU.MANIPULATE:
                            if (selectedObject >= 0)
                            {
                                confirmSelection = true;
                                unloadMainMenu();
                                // loadManipulationMenu();
                                //transfer the selected object on the hand1 marker.
                                Flags.manipulateMode = true;
                                realgroundNode.RemoveChild(sceneObjectsTrans[selectedObject]);
                                Matrix tempMat = sceneObjectsTrans[selectedObject].WorldTransformation;
                                tempMat = tempMat * Matrix.Invert(Matrix.CreateTranslation(tempMat.Translation));
                                sceneObjectsTrans[selectedObject].WorldTransformation = tempMat;
                                hand1MarkerNode.AddChild(sceneObjectsTrans[selectedObject]);
                                confirmstep = 2;
                            }
                            break;
                        case MENU.TRANSLATE:
                            confirmSelection = true;
                            currentManipulation = 1;
                            unloadManipulationMenu();
                            System.Threading.Thread.Sleep(200);
                            loadAxesMenu();
                            confirmstep = 2;
                            break;
                        case MENU.ROTATE:
                            confirmSelection = true;
                            currentManipulation = 2;
                            unloadManipulationMenu();
                            System.Threading.Thread.Sleep(200);
                            loadAxesMenu();
                            confirmstep = 2;
                            break;
                        case MENU.SCALE:
                            confirmSelection = true;
                            currentManipulation = 3;
                            unloadManipulationMenu();
                            System.Threading.Thread.Sleep(200);
                            loadAxesMenu();
                            confirmstep = 2;
                            break;
                        case MENU.XAXIS:
                            confirmSelection = true;
                            unloadAxesMenu();
                            if (currentManipulation == 1)
                            {
                                //trigger translate X
                                if (selectedObject != -1)
                                {
                                    Flags.translateX = true;
                                    Flags.translateY = false;
                                    Flags.translateZ = false;
                                    prevWorldTransform = sceneObjectsTrans[selectedObject].WorldTransformation;
                                }
                            }
                            else if (currentManipulation == 2)
                            {
                                //trigger rotate X
                                Notifier.AddMessage("Rotate X");
                            }
                            else if (currentManipulation == 3)
                            {
                                //trigger scale X
                                if (selectedObject != -1)
                                {
                                    Flags.scaleX = true;
                                    Flags.scaleY = false;
                                    Flags.scaleZ = false;
                                    prevWorldTransform = sceneObjectsTrans[selectedObject].WorldTransformation;
                                }
                            }
                            confirmstep = 2;
                            break;
                        case MENU.YAXIS:
                            confirmSelection = true;
                            unloadAxesMenu();
                            if (currentManipulation == 1)
                            {
                                if (selectedObject != -1)
                                {
                                    //trigger translate Y
                                    Flags.translateX = false;
                                    Flags.translateY = true;
                                    Flags.translateZ = false;
                                    prevWorldTransform = sceneObjectsTrans[selectedObject].WorldTransformation;
                                }
                            }
                            else if (currentManipulation == 2)
                            {
                                //trigger rotate Y
                            }
                            else if (currentManipulation == 3)
                            {
                                //trigger scale Y
                                if (selectedObject != -1)
                                {
                                    Flags.scaleX = false;
                                    Flags.scaleY = true;
                                    Flags.scaleZ = false;
                                    prevWorldTransform = sceneObjectsTrans[selectedObject].WorldTransformation;
                                }
                            }
                            confirmstep = 2;
                            break;
                        case MENU.ZAXIS:
                            confirmSelection = true;
                            unloadAxesMenu();
                            if (currentManipulation == 1)
                            {
                                //trigger translate Z
                                if (selectedObject != -1)
                                {
                                    Flags.translateX = false;
                                    Flags.translateY = false;
                                    Flags.translateZ = true;
                                    prevWorldTransform = sceneObjectsTrans[selectedObject].WorldTransformation;
                                }
                            }
                            else if (currentManipulation == 2)
                            {
                                //trigger rotate Z
                            }
                            else if (currentManipulation == 3)
                            {
                                //trigger scale Z
                                if (selectedObject != -1)
                                {
                                    Flags.scaleX = false;
                                    Flags.scaleY = false;
                                    Flags.scaleZ = true;
                                    prevWorldTransform = sceneObjectsTrans[selectedObject].WorldTransformation;
                                }
                            }
                            confirmstep = 2;
                            break;
                        case MENU.BACK:
                            confirmSelection = true;
                            backSelected();
                            confirmstep = 2;
                            break;

                    }
                    selectedLeaf = -1;
                    confirmSelection = false;
                }
                else if ((Flags.manipulateMode || Flags.createMode) && groundMarkerNode.MarkerFound)
                {
                    try
                    {
                        //This means that the confirm step was perfomed in object creation mode - 
                        //User has confirmed placement of object, hence place the object on ground
                        placeCreatedObjectOnGround();
                        //make object selectable only if this is the first time that object is being created.
                        if (Flags.createMode)
                            makeObjectSelectable();
                        Flags.createMode = false;
                        Flags.manipulateMode = false;
                        confirmstep = 2;
                        loadMainMenu();
                    }
                    catch (Exception)
                    {
                        //Notifier.AddMessage("Multiple parent exception!");
                        //Flags.createMode = false;
                        //Add child should happen only once. Next time the Add child function will give exception.
                        //Do nothing in that case
                    }
                }
                else if (Flags.createWallMode && groundMarkerNode.MarkerFound)
                {
                    try
                    {
                        //This means that the confirm step was perfomed in object creation mode - 
                        //User has confirmed placement of object, hence place the object on ground
                        placeCreatedWallOnGround();

                        Flags.createWallMode = false;
                        confirmstep = 2;
                        loadMainMenu();
                    }
                    catch (Exception)
                    {
                        //Notifier.AddMessage("Multiple parent exception!");
                        //Flags.createMode = false;
                        //Add child should happen only once. Next time the Add child function will give exception.
                        //Do nothing in that case
                    }
                }

            }
            if (groundMarkerNode.MarkerFound)
            {
                if (!setOnce)
                {
                    if (groundReferenceMarkerNode.MarkerFound)
                    {
                        Console.WriteLine("found");
                        //reference marker, transfer to groundmarker
                        Vector3 baseVect = Vector3.Transform(new Vector3(1, 0, 0), groundMarkerNode.WorldTransformation * Matrix.Invert(groundReferenceMarkerNode.WorldTransformation)) -
                                       Vector3.Transform(new Vector3(0, 0, 0), groundMarkerNode.WorldTransformation * Matrix.Invert(groundReferenceMarkerNode.WorldTransformation));
                        baseVect.Normalize();
                        Vector3 temp = new Vector3(1, 0, 0);//ground marker
                        groundRefAngle = System.Math.Acos(Vector3.Dot(temp, baseVect));
                        //groundRefPos = new Vector3(-2 * realgroundTransNode.Translation.X, 0, -realgroundTransNode.Translation.X);
                        groundRefPos = new Vector3(-0.8f * realgroundTransNode.Translation.X, -0.8f * realgroundTransNode.Translation.Y, -1.5f * realgroundTransNode.Translation.X);
                        rootTransNode1.Rotation = Quaternion.CreateFromAxisAngle(Vector3.UnitY, (float)groundRefAngle);
                        rootTransNode1.Translation = groundRefPos;
                    }
                }

                //  if (debug == true) //Sam testing
                //      travelMode = true; //Sam testing
                if (travelMode)
                {
                    if (boxNodeVisible)
                    {
                        racecar.adjustRaceCarVisible(boxNodeVisible);
                        boxNodeVisible = false;
                    }
                    setOnce = true;
                    ////@Neerja: updating user's position on the minimap
                    ////Matrix rotMiniMap = new Matrix();
                    ////youTrans.Translation = ;
                    ////youTrans.
                    //// minimapBaseTrans
                    //double miniAngle = miniUpdateAngle - miniInitAngle;
                    //youTrans.Rotation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, (float)-miniAngle);

                    //if (counter % 2 == 0)
                    //    oldPos = racecar.carNode.WorldTransformation.Translation;
                    //else
                    //    newPos = racecar.carNode.WorldTransformation.Translation;
                    //counter++;

                    //  if (debug == true) //Sam testing
                    //      goto next;
                    if (!carPosAlreadySet)
                    {
                        carPos = Vector3.Transform(racecar.carTransNode.Translation + new Vector3(0, 0, 2), rootTransNode1.WorldTransformation);
                        carPosAlreadySet = true;
                        oldCameraPos = SetCameraPos();
                    }
                    cameraPos = SetCameraPos();
                    if (System.Math.Abs((cameraPos - oldCameraPos).X) <= 1)
                        cameraPos.X = oldCameraPos.X;
                    else
                        oldCameraPos.X = cameraPos.X;
                    if (System.Math.Abs((cameraPos - oldCameraPos).Y) <= 1)
                        cameraPos.Y = oldCameraPos.Y;
                    else
                        oldCameraPos.Y = cameraPos.Y;
                    if (System.Math.Abs((cameraPos - oldCameraPos).Z) <= 1)
                        cameraPos.Z = oldCameraPos.Z;
                    else
                        oldCameraPos.Z = cameraPos.Z;
                    //Vector3 temp = cameraPos;
                    cameraDelt = cameraPos - carPos;
                    rootTransNode2.Translation = cameraDelt;


                next://Sam testing
                    if (miniMapMarkerNode.MarkerFound)
                    {
                        //@Neerja: updating user's position on the minimap
                        //Matrix rotMiniMap = new Matrix();
                        //youTrans.Translation = ;
                        //youTrans.
                        // minimapBaseTrans
                        //double miniAngle = miniUpdateAngle - miniInitAngle;
                        //youTrans.Rotation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ,(float)-miniAngle);

                        //if(counter%2==0)
                        //    oldPos= racecar.carNode.WorldTransformation.Translation;
                        //else
                        //    newPos= racecar.carNode.WorldTransformation.Translation;
                        //counter++;

                        if (!miniMapUpdateOnce)
                        {
                            UpdateMiniMapAngle();
                            miniInitAngle = miniUpdateAngle;
                            miniMapUpdateOnce = true;
                        }
                        else
                        {
                            UpdateMiniMapAngle();
                            double deltAngle;
                            if (miniUpdateAngle <= 85 || miniUpdateAngle >= 95)
                                deltAngle = miniUpdateAngle - miniInitAngle;
                            else
                                deltAngle = 0;
                            miniMapRotateDegree += deltAngle * 0.006 * (float)gameTime.ElapsedGameTime.TotalSeconds;
                            Console.WriteLine(deltAngle);
                            travelSecTransNode.Translation = -racecar.carTransNode.Translation;

                            if (collision1)
                            {
                                miniMapRotateDegree -= deltAngle * 0.006 * 2;
                                collision1 = false;
                            }
                            travelFirstTransNode.Rotation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, (float)miniMapRotateDegree);
                            youTrans1.Rotation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, -(float)miniMapRotateDegree);
                            travelFirstTransNode.Translation = racecar.carTransNode.Translation;


                            /*   Vector3 angularCarTransl = new Vector3(racecar.carTransNode.Translation.X, racecar.carTransNode.Translation.Y, 0);


                               Matrix angularMatPreTransl = new Matrix();
                               angularMatPreTransl = Matrix.CreateTranslation(-angularCarTransl);
                               Matrix angularMatRot = new Matrix();
                               angularMatRot = Matrix.CreateRotationZ((float)miniMapRotateDegree);
                               Matrix angularMatPostTransl = new Matrix();
                               angularMatPostTransl = Matrix.CreateTranslation(angularCarTransl);

                               //real ground transNode
                               Vector3 angularRealGTransl = new Vector3(realgroundTransNode.Translation.X, realgroundTransNode.Translation.Y, 0);
                               Matrix angularMatSuperPreTransl = Matrix.CreateTranslation(angularRealGTransl);

                               Matrix angularRootTrans1 = rootTransNode1.WorldTransformation;
                               Matrix angularRootTrans2 = rootTransNode2.WorldTransformation;
                               // Matrix angularMat = Matrix.Invert(realgroundNode.Physics.PhysicsWorldTransform) * angularMatSuperPreTransl * angularMatPreTransl * angularMatRot * angularMatPostTransl * realgroundNode.Physics.PhysicsWorldTransform;                            

                               Matrix angularMat1 = angularMatSuperPreTransl * angularMatPreTransl * angularMatRot * angularMatPostTransl * angularRootTrans1 * angularRootTrans2;

                               //Matrix angularMat1 = angularMatSuperPreTransl * angularMatPreTransl * angularMatRot;
                               //Console.WriteLine(realgroundNode.Physics.PhysicsWorldTransform.Translation);

                               //Matrix angularMat2 = angularMatPostTransl;

                               ((NewtonPhysics)scene.PhysicsEngine).SetTransform(realgroundNode.Physics, angularMat1);

                               Console.WriteLine(deltAngle);

                               //realgroundNode.Physics.InitialAngularVelocity = new Vector3(0, 0, 10);
                               //((NewtonPhysics)scene.PhysicsEngine).SetTransform(realgroundNode.Physics, angularMat2);

                               //((NewtonPhysics)scene.PhysicsEngine).ApplyAngularVelocity(realgroundNode.Physics, new Vector3(0, 0, (float)deltAngle));

                               for (int i = 0; wallCreator.wallNode[i] != null && i < wallCreator.wallindex; i++)
                               {
                                   Matrix WallMat = Matrix.CreateTranslation(wallCreator.wallTransNode[i].Translation);
                                   Matrix angularWallMat = WallMat * angularMatSuperPreTransl * angularMatPreTransl * angularMatRot * angularMatPostTransl * angularRootTrans1 * angularRootTrans2;
                                   ((NewtonPhysics)scene.PhysicsEngine).SetTransform(wallCreator.wallNode[i].Physics, angularWallMat);
                               }*/

                            //((NewtonPhysics)scene.PhysicsEngine).Update((float)gameTime.ElapsedGameTime.TotalSeconds);

                        }

                        if (speedMarkerNode.MarkerFound)
                        {
                            if (!speedControlUpdateOnce)
                            {
                                UpdateSpeedControlAngle();
                                speedInitAngle = speedUpdateAngle;
                                speedControlUpdateOnce = true;
                                racecar.updateForce((float)deltAngle, 0);
                            }
                            else
                            {
                                float speedAngleThreshold = 5;
                                UpdateSpeedControlAngle();

                                if (Math.Abs(speedUpdateAngle - speedInitAngle) <= speedAngleThreshold)
                                    deltAngle = 0;
                                else
                                    deltAngle = speedUpdateAngle - speedInitAngle;
                                //else if (speedUpdateAngle - speedInitAngle > speedAngleThreshold)
                                //    deltAngle = speedUpdateAngle - (speedInitAngle + speedAngleThreshold);
                                //else
                                //    deltAngle = speedUpdateAngle - (speedInitAngle - speedAngleThreshold);

                                //  Console.WriteLine(deltAngle + "       " + speedUpdateAngle + "       " + speedInitAngle);
                                Notifier.AddMessage(deltAngle + "       " + speedUpdateAngle + "       " + speedInitAngle);

                                //racecar.updateForce(-(float)deltAngle / 4, (float)groundRefAngle);
                                /////add speed translation into travelThirdTransNode.Translation;
                                speedTransChange += (float)gameTime.ElapsedGameTime.TotalSeconds * scaleObjRatio * (float)deltAngle / 2;
                                // travelThirdTransNode.Translation = Vector3.Transform(new Vector3((float)speedTransChange, 0, 0), Matrix.CreateRotationY((float)groundRefAngle));
                                if (collision2)
                                {
                                    collision2 = false;
                                    speedTransChange -= 2 * scaleObjRatio * (float)deltAngle / 2;

                                }
                                travelThirdTransNode.Translation = new Vector3((float)(speedTransChange * Math.Cos(miniMapRotateDegree)), -(float)(speedTransChange * Math.Sin(miniMapRotateDegree)), 0);
                                youTrans2.Translation = new Vector3(-(float)(speedTransChange * Math.Cos(miniMapRotateDegree) * 0.3f), (float)(speedTransChange * Math.Sin(miniMapRotateDegree) * 0.3f), 0);
                            }
                            if (travelThirdTransNode.Translation.Length() > 600)
                            {
                                miniMapRotateDegree = 0;
                                speedTransChange = 0;
                            }
                        }

                        //Console.WriteLine(miniMapMarkerNode.WorldTransformation.Translation - speedMarkerNode.WorldTransformation.Translation);
                    }
                    //MiniMap marker found
                    //else
                    //{
                    //    ;
                    //}
                }
                else
                {
                    setOnce = false;
                    rootTransNode2.Translation = new Vector3(0, 0, 0);
                    if (!boxNodeVisible)
                    {
                        racecar.adjustRaceCarVisible(boxNodeVisible);
                        boxNodeVisible = true;
                    }
                }
            }
            //Console.WriteLine(realgroundNode.Physics.PhysicsWorldTransform.Translation);
            ((NewtonPhysics)scene.PhysicsEngine).Update((float)gameTime.ElapsedGameTime.TotalSeconds);
            if (groundMarkerNode.MarkerFound && (!hand3MarkerNode.MarkerFound || !hand1MarkerNode.MarkerFound))
            {
                Matrix mat = Matrix.CreateTranslation(new Vector3(-1000, -1000, -1000));
                ((NewtonPhysics)scene.PhysicsEngine).SetTransform(groundPointerNode.Physics, mat);
            }

            //Selection and manipulation of objects in scene
            if (groundMarkerNode.MarkerFound && hand3MarkerNode.MarkerFound && !hand1MarkerNode.MarkerFound)
            {
                //selection is possible when menu hand is not visible.
                //Selection is possible for one object only.
                try
                {
                    realgroundNode.AddChild(groundPointerNode);
                }
                catch (Exception)
                {
                    //Add child should happen only once. Next time the Add child function will give exception.
                    //Do nothing in that case
                }
                Vector3 shiftVector = new Vector3(4, -4, 4);
                Matrix mat = Matrix.CreateTranslation(shiftVector) *
                      hand3MarkerNode.WorldTransformation *
                      Matrix.Invert(groundMarkerNode.WorldTransformation);
                ((NewtonPhysics)scene.PhysicsEngine).SetTransform(groundPointerNode.Physics, mat);

            }
            else
            {
                try
                {
                    groundMarkerNode.RemoveChild(groundPointerNode);
                }
                catch (Exception)
                {
                    //Remove child should happen only once. Next time the remove child function will give exception.
                    //Do nothing in that case
                }
            }
            base.Draw(gameTime);
            ((NewtonPhysics)scene.PhysicsEngine).Update((float)gameTime.ElapsedGameTime.TotalSeconds);
        }

        private void makeObjectSelectable()
        {
            ((NewtonPhysics)scene.PhysicsEngine).SetTransform(sceneObjects[objectCount - 1].Physics, sceneObjectsTrans[objectCount - 1].WorldTransformation);
            sceneObjects[objectCount - 1].Physics.ShapeData.Add(objectCount - 1);

            NewtonPhysics.CollisionPair pair = new NewtonPhysics.CollisionPair(sceneObjects[objectCount - 1].Physics, groundPointerNode.Physics);
            ((NewtonPhysics)scene.PhysicsEngine).AddCollisionCallback(pair, objectSelected);
        }
        /////save the camera's position relative to the ground--by Xueying
        private Vector3 SetCameraPos()
        {
            return Matrix.Invert(groundMarkerNode.WorldTransformation).Translation;
            // return groundMarkerNode.WorldTransformation.Translation;
        }

        private void objectSelected(NewtonPhysics.CollisionPair pair)
        {
            objectHighlighterTrans.WorldTransformation = pair.CollisionObject1.PhysicsWorldTransform;
            selectedObject = (int)pair.CollisionObject1.ShapeData[0];
            Notifier.AddMessage(selectedObject.ToString());
        }
        private void placeCreatedObjectOnGround()
        {
            sceneObjectsTrans[selectedObject].Translation = hand1MarkerNode.WorldTransformation.Translation - realgroundNode.WorldTransformation.Translation;
            Matrix hand1WRTground = hand1MarkerNode.WorldTransformation * Matrix.Invert(realgroundTransNode.WorldTransformation * groundNode.WorldTransformation * rootTransNode1.WorldTransformation * rootTransNode2.WorldTransformation);
            Matrix mat = Matrix.CreateScale(sceneObjectsTrans[selectedObject].Scale) * hand1WRTground;
            //mat.Translation = new Vector3(mat.Translation.X,mat.Translation.Y,-mat.Translation.Z);
            sceneObjectsTrans[selectedObject].WorldTransformation = mat;

            hand1MarkerNode.RemoveChild(sceneObjectsTrans[selectedObject]);

            //Add to scene
            realgroundNode.AddChild(sceneObjectsTrans[selectedObject]);

            Material objectMaterial = new Material();
            objectMaterial.Diffuse = Color.Purple.ToVector4();
            sceneObjects[selectedObject].Material = objectMaterial;
            sceneObjects[selectedObject].Physics.Mass = (float)sceneObjectsTrans[selectedObject].Scale.X * 1;

            //Add a copy of same object to minimap

            minimapObjectsTrans[selectedObject].WorldTransformation = sceneObjectsTrans[selectedObject].WorldTransformation;
            if (Flags.createMode)
            {
                minimapObjectsTrans[selectedObject].AddChild(minimapObjects[selectedObject]);
                minimapBaseTrans.AddChild(minimapObjectsTrans[selectedObject]);
            }
            objectHighlighterTrans.WorldTransformation = sceneObjectsTrans[selectedObject].WorldTransformation;
        }
        private void placeCreatedWallOnGround()
        {
            hand1MarkerNode.RemoveChild(wallCreator.wallTransNode[wallCreator.wallindex]);
            wallCreator.createWallOnGround(hand1MarkerNode.WorldTransformation.Translation.X - groundMarkerNode.WorldTransformation.Translation.X, hand1MarkerNode.WorldTransformation.Translation.Y - groundMarkerNode.WorldTransformation.Translation.Y, wallCreator.wallNode[wallCreator.wallindex].Model.MinimumBoundingBox.Max.X - wallCreator.wallNode[wallCreator.wallindex].Model.MinimumBoundingBox.Min.X, wallCreator.wallNode[wallCreator.wallindex].Model.MinimumBoundingBox.Max.Y - wallCreator.wallNode[wallCreator.wallindex].Model.MinimumBoundingBox.Min.Y, wallCreator.wallNode[wallCreator.wallindex].Model.MinimumBoundingBox.Max.Z - wallCreator.wallNode[wallCreator.wallindex].Model.MinimumBoundingBox.Min.Z);
            //wallCreator.placeWallOnGround(hand1MarkerNode.WorldTransformation.Translation.X - groundMarkerNode.WorldTransformation.Translation.X, hand1MarkerNode.WorldTransformation.Translation.Y - groundMarkerNode.WorldTransformation.Translation.Y, wallCreator.wallNode[wallCreator.wallindex].Model.MinimumBoundingBox.Max.X - wallCreator.wallNode[wallCreator.wallindex].Model.MinimumBoundingBox.Min.X, wallCreator.wallNode[wallCreator.wallindex].Model.MinimumBoundingBox.Max.Y - wallCreator.wallNode[wallCreator.wallindex].Model.MinimumBoundingBox.Min.Y, wallCreator.wallNode[wallCreator.wallindex].Model.MinimumBoundingBox.Max.Z - wallCreator.wallNode[wallCreator.wallindex].Model.MinimumBoundingBox.Min.Z, Quaternion.CreateFromRotationMatrix(hand1MarkerNode.WorldTransformation));
            //Add a copy of same object to minimap

            minimapWallsTrans[wallCreator.wallindex].WorldTransformation = wallCreator.wallTransNode[wallCreator.wallindex].WorldTransformation;
            if (Flags.createWallMode)
            {
                minimapBaseTrans.AddChild(minimapWallsTrans[wallCreator.wallindex]);
                minimapWallsTrans[wallCreator.wallindex].AddChild(minimapWalls[wallCreator.wallindex]);
            }
        }
        //Update minimap's rotation angle respect to the ground--by Sheryn
        private void UpdateMiniMapAngle()
        {
            Vector3 miniBaseVector = new Vector3(1, 0, 0);
            Vector3 miniControlVector = Vector3.Transform(new Vector3(1, 0, 0), groundMarkerNode.WorldTransformation * Matrix.Invert(miniMapMarkerNode.WorldTransformation)) -
            Vector3.Transform(new Vector3(0, 0, 0), groundMarkerNode.WorldTransformation * Matrix.Invert(miniMapMarkerNode.WorldTransformation));
            miniControlVector.Z = 0; miniControlVector.Normalize();
            miniUpdateAngle = 180 * System.Math.Acos(Vector3.Dot(miniBaseVector, miniControlVector)) / System.Math.PI;

        }

        private void UpdateSpeedControlAngle()
        {
            Vector3 speedBaseVector = new Vector3(0, 1, 0);
            //  Vector3 speedBaseVector = Vector3.Transform(new Vector3(0, 0, 0), miniMapMarkerNode.WorldTransformation) - Vector3.Transform(new Vector3(0, 0, 0), miniMapMarkerNode.WorldTransformation);
            speedBaseVector.Normalize();
            Vector3 speedControlVector = Vector3.Transform(new Vector3(0, 0, 1), miniMapMarkerNode.WorldTransformation * Matrix.Invert(speedMarkerNode.WorldTransformation)) -
                Vector3.Transform(new Vector3(0, 0, 0), miniMapMarkerNode.WorldTransformation * Matrix.Invert(speedMarkerNode.WorldTransformation));
            //speedControlVector.X = 0; 
            speedControlVector.Normalize();
            speedUpdateAngle = 180 * System.Math.Acos(Vector3.Dot(speedBaseVector, speedControlVector)) / System.Math.PI;
        }

        protected void initMap()
        {
            //outer square           
            //            wallCreator.CreateGroundWall(0, 0, 90f, 2f, 10f);//short edge at origin



            wallCreator.createWallOnGround(0 - 38, 0 + 55, 90f, 2f, 10f);//short edge at origin
            wallCreator.createWallOnGround(0 - 38, -110 + 55, 2f, 110f, 10f);// long edge at origin
            wallCreator.createWallOnGround((float)(9.0 - .2) * wallCreator.iniWallSize - 38, -11 * wallCreator.iniWallSize + 55, 2f, 110f, 10f);//long edge
            wallCreator.createWallOnGround(0 - 38, -110 + 55, 90f, 2f, 10f);//short edge

            //inner edge
            wallCreator.createWallOnGround((float)(9.0 - .2) * wallCreator.iniWallSize / 4 - 38, -8 * wallCreator.iniWallSize + 55, 1f, 50f, 10f);
            wallCreator.createWallOnGround((float)(9.0 - .2) * wallCreator.iniWallSize * 2 / 4 - 38, -11 * wallCreator.iniWallSize + 55, 1f, 50f, 10f);
            wallCreator.createWallOnGround((float)(9.0 - .2) * wallCreator.iniWallSize * 3 / 4 - 38, -8 * wallCreator.iniWallSize + 55, 1f, 50f, 10f);
        }

        protected void initMiniMap()
        {

            //outer square           
            //            wallCreator.CreateGroundWall(0, 0, 90f, 2f, 10f);//short edge at origin

            wallCreator.createWallOnMiniMap(0 - 38, 0 + 55, 90f, 2f, 10f);//short edge at origin
            wallCreator.createWallOnMiniMap(0 - 38, -110 + 55, 2f, 110f, 10f);// long edge at origin
            wallCreator.createWallOnMiniMap((float)(9.0 - .2) * wallCreator.iniWallSize - 38, -11 * wallCreator.iniWallSize + 55, 2f, 110f, 10f);//long edge
            wallCreator.createWallOnMiniMap(0 - 38, -110 + 55, 90f, 2f, 10f);//short edge

            //inner edge
            wallCreator.createWallOnMiniMap((float)(9.0 - .2) * wallCreator.iniWallSize / 4 - 38, -8 * wallCreator.iniWallSize + 55, 1f, 50f, 10f);
            wallCreator.createWallOnMiniMap((float)(9.0 - .2) * wallCreator.iniWallSize * 2 / 4 - 38, -11 * wallCreator.iniWallSize + 55, 1f, 50f, 10f);
            wallCreator.createWallOnMiniMap((float)(9.0 - .2) * wallCreator.iniWallSize * 3 / 4 - 38, -8 * wallCreator.iniWallSize + 55, 1f, 50f, 10f);
        }

        public void CreateNewtonWorld()
        {
            Vector3[] worldPoints = new Vector3[8];
            worldPoints[0] = new Vector3(1000, 1000, 1000);
            worldPoints[1] = new Vector3(1000, -1000, -1000);
            worldPoints[2] = new Vector3(1000, -1000, 1000);
            worldPoints[3] = new Vector3(1000, 1000, -1000);
            worldPoints[4] = new Vector3(-1000, 1000, 1000);
            worldPoints[5] = new Vector3(-1000, -1000, -1000);
            worldPoints[6] = new Vector3(-1000, -1000, 1000);
            worldPoints[7] = new Vector3(-1000, 1000, -1000);
            BoundingBox worldBoundingBox = BoundingBox.CreateFromPoints(worldPoints);
            ((NewtonPhysics)scene.PhysicsEngine).WorldSize = worldBoundingBox;
        }

        //protected void createWall(float TranslationX, float TranslationY, float sizeX, float sizeY, float sizeZ)
        //{
        //    float TranslationXTemp = (sizeX - iniWallSize) / 2 + TranslationX;
        //    float TranslationYTemp = (sizeY - iniWallSize) / 2 + TranslationY;

        //    // Create a geometry node with a model of a box that will be overlaid on
        //    // top of the ground marker array initially. (When the toolbar marker array is
        //    // detected, it will be overlaid on top of the toolbar marker array.)            
        //    wallNode[wallindex] = new GeometryNode("wall" + wallindex);
        //    //wallNode[wallindex].Model = new Box(iniWallSize);
        //    wallNode[wallindex].Model = new Box(sizeX, sizeY, sizeZ);

        //    // Make the sphere models interactable, which means that they
        //    // participate in the physical simulation
        //    wallNode[wallindex].Physics.Interactable = false;
        //    wallNode[wallindex].Physics.Collidable = true;
        //    wallNode[wallindex].Physics.Shape = ShapeType.Box;
        //    wallNode[wallindex].Physics.Mass = 1000f;
        //    wallNode[wallindex].AddToPhysicsEngine = true;

        //    // Make this box model cast and receive shadows
        //    wallNode[wallindex].Model.CastShadows = true;
        //    wallNode[wallindex].Model.ReceiveShadows = true;

        //    // Create a material to apply to the box model
        //    wallMaterial[wallindex] = new Material();
        //    wallMaterial[wallindex].Diffuse = Color.Green.ToVector4();
        //    // wallMaterial[wallindex].Diffuse = Color.TransparentWhite.ToVector4();
        //    wallMaterial[wallindex].Specular = Color.White.ToVector4();
        //    wallMaterial[wallindex].SpecularPower = 10;

        //    wallNode[wallindex].Material = wallMaterial[wallindex];

        //    wallTransNode[wallindex] = new TransformNode("wallTransNode" + wallindex);
        //    wallTransNode[wallindex].Translation = new Vector3(TranslationXTemp, TranslationYTemp, 5f) + new Vector3(-40, 58, -1);

        //    // Add this box model node to the ground marker node
        //    realgroundNode.AddChild(wallTransNode[wallindex]);
        //    wallTransNode[wallindex].AddChild(wallNode[wallindex]);

        //    wallindex++;
        //}




        //keyboard operation for debugging
        private void KeyPressHandler(Keys keys, KeyModifier modifier)
        {
            // Detect key press "t"
            if (keys == Keys.T)
            {
                travelMode = true;
                // rootTransNode2.Translation = cameraDelt;
                //scene.PhysicsEngine.GravityDirection = cameraDelt;
                Matrix gravityMat = new Matrix();
                gravityMat = Matrix.CreateFromAxisAngle(Vector3.UnitY, (float)groundRefAngle);

                scene.PhysicsEngine.GravityDirection = Vector3.Transform(initGravityDir, gravityMat);


                debug = false;
            }

            if (keys == Keys.N)
                travelMode = false;
            //if (keys == Keys.Up)
            //    travelTranslationY += 0.5f;
            if (keys == Keys.Up)
                //((NewtonPhysics)scene.PhysicsEngine).AddForce(realgroundNode.Physics, new Vector3(-80000, 0, 0));
                //((NewtonPhysics)scene.PhysicsEngine).ApplyLinearVelocity(realgroundNode.Physics, new Vector3(-40, 0, 0));
                racecar.updateForce(20, (float)groundRefAngle);
            if (keys == Keys.Down)
                racecar.updateForce(-20, (float)groundRefAngle);
            //((NewtonPhysics)scene.PhysicsEngine).ApplyLinearVelocity(realgroundNode.Physics, new Vector3(40, 0, 0));

        }

        public void CreateVisualGround()
        {
            realgroundNode = new GeometryNode("RealGround");
            realgroundNode.IsOccluder = false;
            realgroundNode.Model = new Box(100 * scaleObjRatio, 120 * scaleObjRatio, 1f);
            realgroundNode.Physics.Shape = ShapeType.Box;
            realgroundNode.Physics.Interactable = true;
            realgroundNode.Physics.Collidable = true;
            realgroundNode.Physics.Mass = 10f;
            realgroundNode.AddToPhysicsEngine = true;
            //realgroundNode.IsOccluder = true;
            realgroundNode.Model.CastShadows = true;
            realgroundNode.Model.ReceiveShadows = true;

            Material realgroundMaterial = new Material();
            realgroundMaterial.Diffuse = Color.Firebrick.ToVector4();
            realgroundMaterial.Specular = Color.White.ToVector4();
            realgroundMaterial.SpecularPower = 10;
            realgroundNode.Material = realgroundMaterial;
            realgroundTransNode = new TransformNode("RealGroundTransNode");
            realgroundTransNode.Translation = new Vector3(50 * scaleObjRatio, -60 * scaleObjRatio, 1);
            //realgroundTransNode.Translation = new Vector3(0, 0, 1);

            travelFirstTransNode = new TransformNode("travelFirstTransNode");
            travelSecTransNode = new TransformNode("travelSecTransNode");
            travelThirdTransNode = new TransformNode();

            groundNode.AddChild(travelFirstTransNode);
            travelFirstTransNode.AddChild(travelSecTransNode);
            travelSecTransNode.AddChild(travelThirdTransNode);
            travelThirdTransNode.AddChild(realgroundTransNode);
            realgroundTransNode.AddChild(realgroundNode);




            //realgroundNode = new GeometryNode("RealGround");
            //realgroundNode.Model = new Box(90, 120, 1f);

            //realgroundNode.Physics.MaterialName = "RealGround";
            //// Make this ground plane collidable, so other collidable objects can collide
            //// with this ground
            //realgroundNode.Physics.Collidable = true;
            //realgroundNode.Physics.Shape = GoblinXNA.Physics.ShapeType.Box;
            //realgroundNode.Physics.Interactable = true;
            //realgroundNode.Physics.Mass = 10f;
            //realgroundNode.AddToPhysicsEngine = true;

            //realgroundNode.Model.CastShadows = true;
            //realgroundNode.Model.ReceiveShadows = true;
            //Material realgroundMaterial = new Material();
            //realgroundMaterial.Diffuse = Color.Firebrick.ToVector4();
            //realgroundMaterial.Specular = Color.White.ToVector4();
            //realgroundMaterial.SpecularPower = 10;
            //realgroundNode.Material = realgroundMaterial;
            //TransformNode realgroundTransNode = new TransformNode();
            //realgroundTransNode.Translation = new Vector3(40, -58, 0);
            //groundMarkerNode.AddChild(realgroundTransNode);
            //realgroundTransNode.AddChild(realgroundNode);



            wallCreator = new WallCreator(realgroundNode, scene, groundMarkerNode, racecar);
        }

        public void createMiniVisualGround()
        {
            float scalingFactor = 0.3f;

            miniRealGroundnode = new GeometryNode("MiniRealGround");
            miniRealGroundnode.IsOccluder = false;
            miniRealGroundnode.Model = new Box(100 * scalingFactor, 120 * scalingFactor, 1f * scalingFactor);
            //miniRealGroundnode.Physics.Shape = ShapeType.Box;
            //miniRealGroundnode.Physics.Interactable = true;
            //miniRealGroundnode.Physics.Collidable = true;
            //miniRealGroundnode.Physics.Mass = 10f;
            //miniRealGroundnode.AddToPhysicsEngine = true;

            miniRealGroundnode.Model.CastShadows = true;
            miniRealGroundnode.Model.ReceiveShadows = true;
            miniRealGroundnode.IgnoreDepth = true;
            Material miniRealgroundMaterial = new Material();
            miniRealgroundMaterial.Diffuse = Color.Yellow.ToVector4();
            miniRealgroundMaterial.Specular = Color.White.ToVector4();
            miniRealgroundMaterial.SpecularPower = 10;
            miniRealGroundnode.Material = miniRealgroundMaterial;
            miniRealgroundTransNode = new TransformNode("miniRealGroundTransNode");

            miniRealgroundTransNode.Translation = new Vector3(50 * scalingFactor, -60 * scalingFactor, 1 * scalingFactor);

            miniMapMarkerNode.AddChild(miniRealgroundTransNode);
            miniRealgroundTransNode.AddChild(miniRealGroundnode);
        }

        public void createCarWallCollision()
        {
            for (int i = 0; i < wallCreator.wallindex; i++)
            {
                wallCreator.carWallCollisionPairs[i] = new NewtonPhysics.CollisionPair(racecar.carNode.Physics, wallCreator.wallNode[i].Physics);
                ((NewtonPhysics)scene.PhysicsEngine).AddCollisionCallback(wallCreator.carWallCollisionPairs[i], carWallCollision);
            }
        }
        private void carWallCollision(NewtonPhysics.CollisionPair pair)
        {
            collision1 = true;
            collision2 = true;
            Console.WriteLine("car wall collision");
        }

    }
}

